function [p]=test()

    function circle(x,y,r)   %画圆函数
        ang=0:0.01:2*pi; 
        xp=r*cos(ang);
        yp=r*sin(ang);
        plot(x+xp,y+yp, '.r');
    end

    function pa = pathOpt(p)
        pa = [];
        while isempty(pa)
            % 改进由RRT*算法生成的路径P
            [num1,p1] = getSampleP(p);
            [num2,p2] = getSampleP(p);
            while num1 == num2  %当num1等于num2时，重新采样
                [num1,p1] = getSampleP(p);
                [num2,p2] = getSampleP(p);
            end
            if ObsFree(p1,p2,20)
                if num1 < num2
                    pa = [p(:,1:num1),p1,p2,p(:,num2+1:end)];
                else
                    pa = [p(:,1:num2),p2,p1,p(:,num1+1:end)];
                end
            end
        end
    end

    function [num, point] = getSampleP(p)
        %在RRT*算法生成的路径P上随机取一点point
        length = size(p, 2);  %获取组成路径的点数（包括终点和起点）
        num = unidrnd(length - 1);  %随机选取一条线段
%         point = [0;0];
        point = p(:,num) + (p(:,num+1) - p(:,num))*rand(1); %在线段上随机选取一点
    end

    function free = ObsFree(node1,node2,n) %n>=2,代表将线段分的段数
        free = 1;
        dx = (node2(1)-node1(1)) / n;
        dy = (node2(2)-node1(2)) / n;
        for i = 1:n
            if ~isObstacleFree([node1(1)+i*dx, node1(2)+i*dy])
                free = 0;
                break
            end
        end
    end

    % if it is obstacle-free, return 1.
    % otherwise, return 0
    function free=isObstacleFree(node_free)   %4
        free = 1;
        for i = 1: length(param.obstacles(:,1))
            obs = param.obstacles(i,:);
%             op1 = [obstacle(1), obstacle(2)];
%             op2 = [op1(1)+obstacle(3), op1(2)];
%             op3 = [op2(1), op1(2) + obstacle(4)];
%             op4 = [op1(1), op3(2)];
            nx = node_free(1);
            ny = node_free(2);
            ha = (nx-obs(1))^2 / obs(3)^2 + (ny-obs(2))^2 / obs(4)^2;
            if (ha <= 1)
                free = 0;
            end
        end 
    end

    param.obstacles =[5,5,5,5;];  %一行代表一个椭圆
    circle(5,5,5)
    hold on
    p = [10  9.804  9.8174   7.5657   2.9383  1.4040  -1.1022  0;
         10  9.842  9.5240  11.6700  10.1356  8.6233   5.8979  0];
%     p = [10,9.069,8.272,8.034,7.049,6.614,6.127,5.606,4.919,4.074,3.765,3.057,-0.345,-0.905,0;
%         10,10.401,10.646,10.534,10.413,10.590,10.917,11.126,11.174,11.485,11.113,11.060,6.119,5.931,0];
    p = fliplr(p);  % 左右翻转矩阵p
    plot(p(1,:),p(2,:),'--ob','LineWidth',2)
    axis equal
%     new_p = pathOpt(p);
%     plot(new_p(1,:),new_p(2,:),'--og','LineWidth',1,'MarkerFaceColor','r')

    
    function refinedP = refinePath(p,u_max)
        % 验证方向角变化速度是否在规定的最大变化速度之内
        refinedP = [];
        while isempty(refinedP)
            p = pathOpt(p);
            if steeringEval(p,u_max) || length(p)>200
                refinedP = p;
            end
        end
    end

    function state = steeringEval(p,u_max)
        % p的格式i：[p1，p2，p3]_{2x3}，其中p1,p2,p3是相邻的边的三个端点
        state = 1;
        for i = 1:size(p,2)-2
            pf= cross([p(:,i);0]-[p(:,i+1);0],[p(:,i);0]-[p(:,i+2);0]);
            if all(pf == 0) % 如果三点共线，跳出当前循环
                continue;
            else
                R = circleCenter([p(:,i);0],[p(:,i+1);0],[p(:,i+2);0]);
            end
            K = 1 / R;   % K是曲率
            u = K;
            if u > u_max
                state = 0;
                break;
            end
        end
    end

    function r = circleCenter(p1, p2, p3)
        % 在网上找的程序
        % CircleCenter(p1, p2, p3) 根据三个空间点，计算出其圆心，再求得R
        % p1,p2,p3:三个空间点在网上找的程序
        % 圆的法向量
        pf= cross(p1-p2, p1-p3);
%         if all(pf == 0)
%         	error('三个点不能共线！！');
%         end
        % 两条线段的中点，之后需要求中垂线
        p12 = (p1 + p2)/2;
        p23 = (p2 + p3)/2;
        % 求两条线的中垂线
        p12f = cross(pf, p1-p2);
        p23f = cross(pf, p2-p3);
        % 求在中垂线上投影的大小
        ds = ( (p12(2)-p23(2))*p12f(1) - (p12(1)-p23(1))*p12f(2) ) /...
        	( p23f(2)*p12f(1) - p12f(2)*p23f(1) );
        % 得出距离
        centre = p23 + p23f .* ds;
        r = norm(centre-p1);
    end
    
%     circleCenter([-1.1022;5.8979;0],[1.4040;8.6233;0],[2.9383;10.1356;0])
    u_max = 0.2;
%     isChangeRateGood(p, u_max)
    finedP = refinePath(p,u_max);
    plot(finedP(1,:),finedP(2,:),'-o','LineWidth',2,...
        'MarkerFaceColor','y')
    title(['\rm\it\fontname{times new roman}rrt*\rm\fontname{宋体}和',...
        '\it\fontname{times new roman}refined rrt*',...
        '\rm\fontname{宋体}某次仿真结果'])
    xlabel('\it\fontname{times new roman}x')
    ylabel('\it\fontname{times new roman}y')
%     circleCenter([0.0251,5.5093,0],[0.0260,5.5196,0],[0.0424,5.6514,0])
end